using System;
using System.Collections;
using System.Drawing;
using FIRADriverLibrary;
using GeometryCSharp;
using System.IO;

namespace AGHFiraPlayer
{
	public abstract class PotencialDriver
	{
		public static ArrayList angleLocks;
		public static Point2D checkPoint;

		public const double wheelsHalfDistance = 0.034;
		public const double robotCollisionAvoidanceDistance = 0.2;
        public const double speedUpDistance = 0.2;

		public static double maxVelocity = 0.6;
		public static double tinyVelocity = 0.2;	//below robot will change direction if needed, above will turn
		public static double freeCurveFactor = 0.6;
		public static double collisionDetectionTime = 0.5;

     //   public static StreamWriter sw = new StreamWriter("C:\\file.txt");
		//public static double collisionAvoidanceFactor = 2;

        public static void DriveVelocity(FIRARobotState robot, double x, double y, double velocity)
        {
            double linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;
            double deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;

            checkPoint = CalculateCollisionFreeCheckPoint(robot, x, y, Math.Abs(collisionDetectionTime * linearVel) + robotCollisionAvoidanceDistance);
            linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;	//might have chaged
            deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;
            if (checkPoint.x != 0.0 || checkPoint.y != 0.0)
            {
                double toCheckPointDistance = Vector2D.GetLength(robot.x - checkPoint.x, robot.y - checkPoint.y);
                double fromCheckPointDistance = Vector2D.GetLength(x - checkPoint.x, y - checkPoint.y);

                CalculateControlNoObstacles(robot, checkPoint.x, checkPoint.y, velocity, ref linearVel, ref deltaVel);
            }
            else
            {
                CalculateControlNoObstacles(robot, x, y, velocity, ref linearVel, ref deltaVel);
                angleLocks.Clear();
            }

            robot.leftSpeed = linearVel + deltaVel;
            robot.rightSpeed = linearVel - deltaVel;
        }

        public static void CalculateControlNoObstacles(FIRARobotState robot, double x, double y, double velocity, ref double linearVel, ref double deltaVel)
        {
            Vector2D robotVersor = new Vector2D(robot.angle);
            Vector2D targetVector = new Vector2D(x - robot.x, y - robot.y);

            double angleRobotTarget = Vector2D.AngleBetween(robotVersor, targetVector);
       //     sw.WriteLine("kat WAZNE " + angleRobotTarget);

            //sw.WriteLine("kat WAZNE " + angleRobotTarget);
            linearVel = Math.Sign(Math.Cos(angleRobotTarget)) * Math.Min(maxVelocity, velocity);
            deltaVel = freeCurveFactor * Math.Sin(angleRobotTarget) * linearVel;
      //      sw.WriteLine("linear I " + linearVel);
      //      sw.WriteLine("delta I " + deltaVel);
            //sw.WriteLine("linear I " + linearVel);
            //sw.WriteLine("delta I " + deltaVel);
            if (targetVector.Length < robotCollisionAvoidanceDistance)	// sharpen turn if close to target
            {
                double sharpenFactor = (robotCollisionAvoidanceDistance - targetVector.Length) / robotCollisionAvoidanceDistance;
                deltaVel *= 1 + sharpenFactor;
                linearVel *= 0.5 + 0.5 * (1 - sharpenFactor);
         //       sw.WriteLine("linear 2 " + linearVel);
         //       sw.WriteLine("delta 2 " + deltaVel);
                //sw.WriteLine("linear 2 " + linearVel);
                //sw.WriteLine("delta 2 " + deltaVel);
            }
            
            if (Math.Abs(robot.leftSpeed + robot.rightSpeed) / 2 > tinyVelocity)	// robot at motion
            {
                if (Math.Sign(robot.leftSpeed + robot.rightSpeed) != Math.Sign(linearVel))	// invert motion
                {
                    linearVel = -linearVel;
                    deltaVel = Math.Sign(Math.Sin(angleRobotTarget)) * freeCurveFactor * linearVel;
       //             sw.WriteLine("linear 3 " + linearVel);
       //             sw.WriteLine("delta 3 " + deltaVel);
                    //sw.WriteLine("linear 3 " + linearVel);
                    //sw.WriteLine("delta 3 " + deltaVel);
                }
            }
        }

		public static void DriveETA (RobotController robotController, double x, double y, double eta)
		{
			FIRARobotState robot = robotController.robotState;
			double linearVel = (robot.currentLeftSpeed + robot.currentRightSpeed) / 2;
			double deltaVel = (robot.currentLeftSpeed - robot.currentRightSpeed) / 2;

			checkPoint = CalculateCollisionFreeCheckPoint(robot, x, y, Math.Abs(collisionDetectionTime * linearVel) + robotCollisionAvoidanceDistance);
			linearVel = (robot.currentLeftSpeed + robot.currentRightSpeed) / 2;//might have chaged
			deltaVel = (robot.currentLeftSpeed - robot.currentRightSpeed) / 2;
			if (checkPoint.x != 0.0 || checkPoint.y != 0.0)
			{
				double toCheckPointDistance = Vector2D.GetLength(robot.x - checkPoint.x, robot.y - checkPoint.y);
				double fromCheckPointDistance = Vector2D.GetLength(x - checkPoint.x, y - checkPoint.y);

				CalculateControlNoObstaclesETA(robot, checkPoint.x, checkPoint.y, eta * (toCheckPointDistance / (toCheckPointDistance+fromCheckPointDistance))
					, ref linearVel, ref deltaVel);
			}
			else
			{
				CalculateControlNoObstaclesETA(robot, x, y, eta, ref linearVel, ref deltaVel);
				angleLocks.Clear();
			}

			robot.leftSpeed = linearVel + deltaVel;
			robot.rightSpeed = linearVel - deltaVel;
        }

		public static void DriveBlind(RobotController robotController, double x, double y, double eta)
		{
			FIRARobotState robot = robotController.robotState;
			double linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;
			double deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;
			CalculateControlNoObstaclesETA(robot, x, y, eta, ref linearVel, ref deltaVel);
			robot.leftSpeed = linearVel + deltaVel;
			robot.rightSpeed = linearVel - deltaVel;
		}


   
		public static void CalculateControlNoObstaclesETA(FIRARobotState robot, double x, double y, double eta, ref double linearVel, ref double deltaVel)
		{
			Vector2D robotVersor = new Vector2D(robot.angle);
			Vector2D targetVector = new Vector2D(x-robot.x, y-robot.y);

            double angleRobotTarget = Vector2D.AngleBetween(robotVersor, targetVector);

			linearVel = Math.Sign(Math.Cos(angleRobotTarget)) *
				 Math.Min(maxVelocity, targetVector.Length / eta);
			deltaVel = freeCurveFactor * Math.Sin(angleRobotTarget) * linearVel;

			if (targetVector.Length < robotCollisionAvoidanceDistance)	// sharpen turn if close to target
			{
				deltaVel *= 1 + (robotCollisionAvoidanceDistance - targetVector.Length) / robotCollisionAvoidanceDistance;
			}

			if (Math.Abs(robot.leftSpeed + robot.rightSpeed) / 2 > tinyVelocity)	// robot at motion
			{
				if (Math.Sign(robot.leftSpeed + robot.rightSpeed) != Math.Sign(linearVel))	// invert motion
				{
					linearVel = -linearVel;
					deltaVel = Math.Sign(Math.Sin(angleRobotTarget)) * freeCurveFactor * linearVel;
				}
			}
		}

		private static Point2D CalculateCollisionFreeCheckPoint(FIRARobotState robot, double x, double y, double lookupDistance)
		{
//			Vector2D robotVector = new Vector2D(robot.angle);
//			double robotAngle = robot.angle * Math.Sign(robot.leftSpeed + robot.rightSpeed);

			Vector2D targetVector = new Vector2D(x-robot.x, y-robot.y);
			Vector2D robotVector = targetVector + (new Vector2D(robot.angle) * (lookupDistance  * Math.Sign(robot.leftSpeed + robot.rightSpeed)));
			double robotAngle = robotVector.Angle;

			angleLocks = new ArrayList();
			Vector2D robotObstacleRobotVector;
            		
			// robots locks
			foreach (FIRARobotState obstacleRobot in Predicator.firaDriver.firaRobots)
			{
				if (obstacleRobot == robot)
					continue;

                robotObstacleRobotVector = new Vector2D(obstacleRobot.x - robot.x, obstacleRobot.y - robot.y);
				double obstacleDistance = robotObstacleRobotVector.Length;
				if (obstacleDistance > lookupDistance)
					continue;

				double obstacleAngle = robotObstacleRobotVector.Angle;
				double obstacleWidthHalfAngle = Math.Atan(robotCollisionAvoidanceDistance / obstacleDistance);
				if (obstacleDistance < robotCollisionAvoidanceDistance)
				obstacleWidthHalfAngle *= 1 + (robotCollisionAvoidanceDistance - obstacleDistance) / robotCollisionAvoidanceDistance;
				angleLocks.Add(new AngleLock(obstacleAngle, obstacleWidthHalfAngle, obstacleDistance));
			}

			//walls locks
            double wallDistance = Math.Max(0.01, robot.x - Predicator.pitchArea.Left);
			if (wallDistance > 0.0 && wallDistance < lookupDistance)
			{				
				angleLocks.Add(new AngleLock(Math.PI, Math.Acos(wallDistance/lookupDistance), wallDistance));
			}
            wallDistance = Math.Max(0.01, Predicator.pitchArea.Right - robot.x);
			if (wallDistance > 0.0 && wallDistance < lookupDistance)
			{		
				angleLocks.Add(new AngleLock(0.01, Math.Acos(wallDistance/lookupDistance), wallDistance));
			}
            wallDistance = Math.Max(0.01, robot.y - Predicator.pitchArea.Top);
			if (wallDistance < lookupDistance)
			{				
				angleLocks.Add(new AngleLock(3*Math.PI/2, Math.Acos(wallDistance/lookupDistance), wallDistance));
			}
            wallDistance = Math.Max(0.01, Predicator.pitchArea.Bottom - robot.y);
			if (wallDistance < lookupDistance)
			{		
				angleLocks.Add(new AngleLock(Math.PI/2, Math.Acos(wallDistance/lookupDistance), wallDistance));
			}

			//remove too far locks, stop if a very close lock is present
			for (int i = 0 ; i < angleLocks.Count ; i++)
			{
				double obstacleDistance = ((AngleLock)angleLocks[i]).lockVector.Length;
				if (obstacleDistance > targetVector.Length)	 
				{
					angleLocks.RemoveAt(i);
					i--;
				}
				else if (obstacleDistance < robotCollisionAvoidanceDistance /2)
				{
					robot.leftSpeed = 0.0;
					robot.rightSpeed = 0.0;
				}
			}

			///join locks
			for (int i = 0 ; i < angleLocks.Count ; i++)
			{
				AngleLock angleLock = (AngleLock)angleLocks[i];
				if (angleLock.lockVector.Length > targetVector.Length)	 
				{
					angleLocks.RemoveAt(i);
					i--;
				}
				for (int j = i + 1 ; j < angleLocks.Count ; j++)
				{
					if (angleLock.TryJoin((AngleLock)angleLocks[j]))
					{
						angleLocks.RemoveAt(j);
						j--;
					}
				}
			}

			// find and go around
			double newAngle = robotAngle;
			foreach (AngleLock angleLock in angleLocks)
			{
					
				newAngle = angleLock.GetClosestFreeAngle(newAngle);
				if (newAngle != robotAngle)
				{
					if (angleLock.lockVector.Length < robotCollisionAvoidanceDistance) //prevent keeping motion direction
					{						
						robot.leftSpeed = 0.0;
						robot.rightSpeed = 0.0;
					}
					return new Point2D(robot.x + angleLock.lockVector.Length * Math.Cos(newAngle),
						robot.y + angleLock.lockVector.Length * Math.Sin(newAngle));
				}
			}
			return new Point2D(0.0, 0.0);

		}












		// simple, good for separated obsacles
//		private static void Avoid(FIRARobotState robot, ref double linearVel, ref double deltaVel)
//		{
//			Vector2D robotVector = new Vector2D(robot.angle) * linearVel * collisionDetectionTime;
//			double robotVectorLength = robotVector.Length;
//			Vector2D robotObstacleRobotVector;
//
//			double influence = 0.0;
//
//			foreach (FIRARobotState obstacleRobot in firaDriver.firaRobots)
//			{
//				if (obstacleRobot == robot)
//					continue;
//
//				robotObstacleRobotVector.X = obstacleRobot.x - robot.x;
//				robotObstacleRobotVector.Y = obstacleRobot.y - robot.y;
//
//				double angleRobotRobotObstacle = Vector2D.AngleBetween(robotVector, robotObstacleRobotVector);
//
//				if (Math.Cos(angleRobotRobotObstacle) < 0)
//					continue;
//
//				if (robotObstacleRobotVector.Length / Math.Cos(angleRobotRobotObstacle) > robotVectorLength)
//					continue;
//
//				influence += (robotVectorLength - robotObstacleRobotVector.Length) * Math.Sign(Math.Sin(angleRobotRobotObstacle)) * Math.Cos(angleRobotRobotObstacle);
//			}
//
//
//			deltaVel -= collisionAvoidanceFactor *(influence / robotVectorLength);
//		}
	}

    public class AngleLock
    {
        public Vector2D lockVector;
        public double halfAngle;

        public AngleLock(double lockCenterAngle, double halfAngle, double distance)
        {
            this.lockVector = new Vector2D(lockCenterAngle);
            this.lockVector *= distance;
            this.halfAngle = halfAngle;
        }

        public bool TryJoin(AngleLock secondLock)
        {
            double angleBetween = Vector2D.AngleBetween(lockVector, secondLock.lockVector);

            if (Math.Abs(angleBetween) > halfAngle + secondLock.halfAngle)
                return false;

            /// the second range can exceed the first from one or both sides
            double wider_big = Math.Abs(angleBetween) + secondLock.halfAngle - halfAngle;
            double wider_small = -Math.Abs(angleBetween) + secondLock.halfAngle - halfAngle;
            if (wider_big > 0)
            {
                if (wider_small < 0)
                    wider_small = 0;
                double length = Math.Min(lockVector.Length, secondLock.lockVector.Length);
                lockVector = new Vector2D(lockVector.Angle + Math.Sign(angleBetween) * (wider_big - wider_small) / 2);
                this.lockVector *= length;
                this.halfAngle += (wider_big + wider_small) / 2;
                this.halfAngle = Math.Min(this.halfAngle, Math.PI);
            }
            return true;
        }

        public double GetClosestFreeAngle(double angle)
        {
            Vector2D robotVersor = new Vector2D(angle);
            double angleBetween = Vector2D.AngleBetween(lockVector, robotVersor);
            if (Math.Abs(angleBetween) > this.halfAngle)
                return angle;

            return lockVector.Angle + Math.Sign(angleBetween) * this.halfAngle;
        }
    }
}
